﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MiddleLayer;
using System.ComponentModel;
using System.Threading;
using System.Timers;
using System.Text.Json;
using System.Diagnostics;

namespace WingbotDriver
{
    class WingBot
    {
        #region "local type"
        public enum PidMode_
        {
            PID_MOVE,
            PID_SPIN,
            PID_SHIFT
        }

        /// <summary>
        /// RunningMode指的都是组队情况下的运行模式
        /// </summary>
        public enum RunningMode
        {
            SEPARATE_MODE,          //0，分体模式，旋转的时候设置DoubleCar，直行关闭DoubleCar
            MERGE_MODE,             //1，协作模式
            PARKING_MODE,           //2，泊车模式，泊车模式下Pid必须禁用，从泊车模式到协作模式必须要把汽车抬起来
            DISMISS_MODE,           //3，解散模式，主从车都由后端单独控制。0~2模式下从车的行为仅受主车控制
            OFFLINE_MODE            //4，断开与stm32的连接
        }

        public enum TeamPosition
        {
            UNKOWN = 0,             //未分配
            FRONT = 1,
            BACK = 2
        }

        public enum RaiseStates
        {
            UNKOWN = 0,             //未分配
            Up = 1,
            MovingUp = 2,
            Down = 3,
            MovingDown = 4
        }

        public enum ArmStates
        {
            UNKOWN = 0,             //未分配
            Open = 1,
            Openning = 2,
            Closed = 3,
            Closing = 4
        }
        #endregion

        #region "fields"

        public WingPort port;
        public BatteryPort battery;
        WingBot slave;
        WebServer server;
        Thread PidThread;
        double _CommandVel;
        double GoToDx;                            //微调的距离
        double GoToDy;                            //微调的y距离
        //double GoToDTh;
        short WheelSpinRpm;
        double VelErr;
        short _CommandSpinRpm = 0;
        short SpinRpmErr = 0;
        const short SpinAcce = 5;
        const int SetOdomFromTmpTf = 1;
        const double AngleFine = 3;
        const double VelAcce = 0.02;
        const double PidMaxSpinRpm = 270;         //PID控制的时候最大旋转速度
        const double _TeamDistance = 2.5;         //主从车中心点2.5米
        const double MinTeamDistance = 2.0;       //双车组队的时候允许的两车最小距离
        const double SpinSlowRatio = 10.0;         //双车旋转的时候转的快的减速因子
        const uint NavigationFinishId = 205;      //导航任务完成主动上报
        const double MaxGoToVel = 0.1;
        const double MaxGoToSpinRpm = 50;

        private struct Point
        {
            public double x;
            public double y;
            public double yaw;
        }

        /// <summary>
        /// 全局的坐标，ClearOdom只清除Stm32的坐标，也就是本地坐标
        /// 这样的话TmpSetOdomTf似乎没用了，原先设想的是特殊情况下暂时修改坐标系，改完后要恢复
        /// </summary>
        private Point GlobalPosition = new();

        /// <summary>
        /// 局部坐标系的原点
        /// </summary>
        private Point LocalOrigin = new();

        CommonPid LinePid = new();
        CommonPid YawPid = new();
        CommonPid TurnPid = new();
        CommonPid MovePid = new();
        PidErr MovePidErr = new();
        PidErr LinePidErr = new();
        PidErr YawPidErr = new();
        PidErr TurnPidErr = new();

        #endregion
        [Description("PID控制时候的最大直行速度")]
        public double PidMaxVel { get; private set; } = 0.4;             //PID控制时候的最大直行速度
        [Description("是否手动停止Pid导航")]
        public bool ManualStop { get; private set; }
        [Description("底层驱动IP地址")]
        public string LowLayerIp { get; set; }
        [Description("底层驱动端口号")]
        public int LowLayerPort { get; set; }
        [Description("Pid行驶的时候维持的角度")]
        public int KeepYaw { get; set; }
        [Description("临时修改KeepYaw的偏差")]
        public double KeepYawOffset { get; set; } = 0;
        [Description("是否在执行Pid任务")]
        public bool PidActionBusy { get; private set; }
        [Description("运行在合并模式还是分体模式")]
        public RunningMode RunMode { get; set; }
        [Description("是否已经运行在合并模式(也叫协作模式)")]
        public bool Merged
        {
            get
            {
                if (slave is null)
                    return false;
                return (slave.RunMode == RunningMode.MERGE_MODE) &&
                    (this.RunMode == RunningMode.MERGE_MODE)
                    ;
            }
        }
        [Description("当前的控制模式")]
        public uint CtrlMode
        {
            get
            {
                if(Merged)
                {
                    if (port.RobotState.ctrl_mode != slave.port.RobotState.ctrl_mode)
                        return WingPort.CtrlModeNotReady;
                }
                return port.RobotState.ctrl_mode;
            }
        }
        [Description("是否使用Pid控制模式")]
        public bool PidEnable { get; private set; }

        private double _TargetVel;
        [Description("Pid控制的目标速度")]
        public double TargetVel
        {
            get
            {
                return _TargetVel;
            }
        }

        private double _RetardingFactor;
        [Description("识别到障碍物的减速因子")]
        public double RetardingFactor
        {
            get
            {
                return Math.Max(0, Math.Min(_RetardingFactor, 1.0));
            }
            set { _RetardingFactor = value; }
        }

        [Description("从车与主车的角度差，主车yaw减去从车yaw")]
        public double AngleDiff { get; private set; }

        [Description("设置当前沿着哪个轴的方向直行")]
        public string KeepAxis { get; private set; }
        [Description("沿Y轴直行时X的坐标")]
        public double CenterX { get; private set; }
        [Description("沿X轴直行时Y的坐标")]
        public double CenterY { get; private set; }
        [Description("Pid控制的模式，直行或自转")]
        public PidMode_ PidMode { get; private set; }
        [Description("斜移的时候的轮子角度")]
        public double ShiftAngle { get; private set; }
        [Description("是否组队")]
        public bool TeamEnable
        {
            get
            {
                return port.RobotState.team_info.enable > 0;
            }
        }
        [Description("组队距离，两车中心点距离")]
        public double TeamDistance { get; private set; }
        [Description("组队的时候是主车还是从车")]
        public TeamPosition TeamRole { get; set; }
        [Description("最近在执行的任务Id")]
        public uint TaskId { get; private set; }
        [Description("状态刷新计数")]
        public uint StateUpdateCnt { get; private set; }
        [Description("抱臂状态")]
        public int ArmState { get => port.RobotState.arm_state; }
        [Description("抬升状态")]
        public int RaiseState { get => port.RobotState.raise_state; }
        [Description("斜移的时候的速度")]
        public double CurrentShiftVel { get => port.RobotState.wheel_mps; }
        [Description("直行时候的速度")]
        public double CurrentMoveVel { get => port.RobotState.wheel_mps; }
        [Description("旋转的时候的速度")]
        public short CurrentSpinRpm  { get => port.RobotState.odom.v_angular_z; }

        public WingBot()
        {
            port = new();
            server = new();
            battery = new();
            PidMode = PidMode_.PID_MOVE;
            RunMode = RunningMode.OFFLINE_MODE;
            TeamRole = TeamPosition.UNKOWN;
            LowLayerIp = "127.0.0.1";
            LowLayerPort = 5000;
            TeamDistance = 2.5;
            PidActionBusy = false;
            RetardingFactor = 1.0;
            SetKeepAxis("X", 0.0);
        }

        public void Dispose()
        {
            port.Close();
            Console.WriteLine("Waiting WebPort Tasks Finish.");
            while(true)
            {
                Thread.Sleep(10);
                if(!port.CmdFlags.PollingState)
                {
                    Console.WriteLine("WebPort Fully Disposed.");
                    break;
                }
            }
            RunMode = RunningMode.OFFLINE_MODE;
        }

        /// <summary>
        /// 绑定从车
        /// </summary>
        /// <param name="b"></param>
        /// <returns></returns>
        public void BindSlave(WingBot b)
        {
            slave = b;
        }

        public void UnbindSlave()
        {
            if ( (!(slave is null)) && (TeamRole == TeamPosition.FRONT))
            {
                //断开了之后Pid循环也就退出了
                Console.WriteLine("Master Unbind Slave.");
                slave.Dispose();
            }

            if(TeamRole == TeamPosition.FRONT)
                this.RunMode = RunningMode.SEPARATE_MODE;
        }

        public void BeginListen()
        {
            var t = new Thread(() => server.Listen(19001));
            server.OnJsonMessage = OnJsonMessage;
            t.Start();
        }

        public void Begin()
        {
            port.RunWatchDog();
            while (true)
            {
                Console.WriteLine($"Trying to Connect LowLayer Server {LowLayerIp}:{LowLayerPort}...");
                if (Connect())
                {
                    Console.WriteLine($"Connect {LowLayerIp}:{LowLayerPort} Successfully");
                    break;
                }
            }

            if (port.Ready)
            {
                RunMode = RunningMode.SEPARATE_MODE;
                SetTargetSpeed(0.0);
                port.RequestVersion();
                port.RequestRobotState();
                ManualStop = false;
                StartPid();
            }
        }

        public bool BeginNoPid(int timeout)
        {
            if (port.Ready)
            {
                Console.WriteLine("Already Connected, Plese Check System State.");
                return true;
            }

            if (!Connect(timeout))
            {
                Console.WriteLine($"Can't Connect {LowLayerIp}:{LowLayerPort}");
                return false;
            }

            if (port.Ready)
            {
                SetTargetSpeed(0.0);
                port.RequestVersion();
                port.RequestRobotState();
                ManualStop = false;
            }
            Console.WriteLine($"Connect {LowLayerIp}:{LowLayerPort} Successfully");
            return true;
        }

        private bool Connect(int timeout = 1500)
        {
            bool ret = port.TcpConnect(LowLayerIp, LowLayerPort, valTimeOut: timeout);
            return ret;
        }

        public void Loop()
        {
            while (true)
            {
                Thread.Sleep(1000);
                if(Merged)
                {
                    if(!slave.port.Ready)
                    {
                        Console.WriteLine("In Merge Mode, But Slave Offline, Reconnecting...");
                        slave.BeginNoPid(1000);
                    }
                }
                if(port.Booting)
                {
                    port.Booting = false;
                    KeepYaw = NormaliseKeepYaw(port.RobotState.odom.yaw);
                    Console.WriteLine($"KeepYaw Updated to {KeepYaw}");
                }
            }
        }

        private string GetByteString(byte[] b)
        {
            string ret = "";
            for (int i = 0; i < b.Length; i++)
                ret += b[i].ToString("X2");
            return ret;
        }

        private double ReduceAccuracy(double v)
        {
            return ((int)(v * 1000.0)) / 1000.0;
        }

        private void RefreshGlobalPosition()
        {
            Func<double, double> reset_angle = (double yaw) =>
            {
                while (Math.Abs(yaw) > 180)
                {
                    yaw = yaw - Math.Sign(yaw) * 360;
                }
                return yaw;
            };
            double dx = port.RobotState.odom.x / 1000.0;
            double dy = port.RobotState.odom.y / 1000.0;
            double dth = reset_angle(LocalOrigin.yaw + port.RobotState.odom.yaw);
            double rad = LocalOrigin.yaw / 180.0 * Math.PI;
            GlobalPosition.x = LocalOrigin.x + dx * Math.Cos(rad) - dy * Math.Sin(rad);
            GlobalPosition.y = LocalOrigin.y + dx * Math.Sin(rad) + dy * Math.Cos(rad);
            GlobalPosition.yaw = dth;
        }

        public bool IsAngleFine()
        {
            bool ret = _IsAngleFine();
            if(Merged)
            {
                ret = ret && slave._IsAngleFine();
            }
            return ret;
        }

        public bool _IsAngleFine()
        {
            bool ret = (Math.Abs(port.RobotState.angleErr[0]) < AngleFine &&
                        Math.Abs(port.RobotState.angleErr[1]) < AngleFine &&
                        Math.Abs(port.RobotState.angleErr[2]) < AngleFine &&
                        Math.Abs(port.RobotState.angleErr[3]) < AngleFine);
            return ret;
        }

        private int NormaliseKeepYaw(double yaw)
        {
            int ret = 0;
            if(yaw >=-45 && yaw <45)
            {
                ret = 0;
            }
            else if(yaw >= 45 && yaw < 135)
            {
                ret = 90;
            }
            else if(yaw >= 135 || yaw < -135 )
            {
                ret = 180;
            }
            else if(yaw >= -135 && yaw < -45)
            {
                ret = -90;
            }
            else
            {
                Console.WriteLine($"Illeagal Input Yaw: {yaw}");
            }
            return ret;
        }

        public void OnJsonMessage(string data)
        {
            WingJson wing_json;
            try
            {
                wing_json = JsonSerializer.Deserialize<WingJson>(data);
                /* 获取状态和设置速度的消息太多就不打印了 */
                if (wing_json.id != 1 && wing_json.id != 6)
                    Console.WriteLine(data);
            }
            catch (Exception e)
            {
                Console.WriteLine(e.Message);
                return;
            }
            bool ret;
            switch (wing_json.id)
            {
                case 1:
                    var client_json = JsonSerializer.Deserialize<ClientJson>(data);
                    RetardingFactor = client_json.data.retarding;
                    AngleDiff = client_json.data.AngleDiff;
                    /* 这是未来的扩展方向，因为如果要动态调整Pid维持的方向，
                     * 除了修改角度还需要修改直线方程，不然仅仅维持角度是不能走斜线的
                     */
                    KeepYawOffset = client_json.data.KeepYawOffset;
                    PidMaxVel = client_json.data.MaxVel > 0 ? client_json.data.MaxVel : 0.4;
                    if(client_json.data.EnableTmpTf)
                    {
                        port.ActiveTmpTf();
                        port.TmpSetOdomTf(
                            client_json.data.TmpX,
                            client_json.data.TmpY,
                            client_json.data.TmpYaw
                            );
                    }
                    else
                    {
                        port.TmpClearOdom();
                        port.RecoverTmpTf();
                    }
                    ServerRetrunState(wing_json.cnt);
                    break;
                case 2:
                    var move_json = JsonSerializer.Deserialize<MoveToPointJson>(data);
                    ret = MoveToPointTask(move_json);
                    ServerConfirm(wing_json, ret);
                    break;
                case 3:
                    var mode_json = JsonSerializer.Deserialize<ModeJson>(data);
                    ret = SetRunMode((RunningMode)mode_json.data.RunMode, mode_json.data.TeamDistance);
                    ServerConfirm(wing_json, ret);
                    break;
                case 4:
                    // 设置增量或清空临时坐标，设置增量坐标时不更新坐标系
                    ServerConfirm(wing_json, true);
                    var odom_json = JsonSerializer.Deserialize<AddOdomJson>(data);
                    ChangeOdom(odom_json);
                    break;
                case 5:
                    var pid_json = JsonSerializer.Deserialize<PidJson>(data);
                    ret = TogglePid(pid_json.data);
                    ServerConfirm(wing_json, ret);
                    break;
                case 6:
                    var speed_json = JsonSerializer.Deserialize<SpeedJson>(data);
                    SetSpeed(speed_json);
                    ServerConfirm(wing_json, true);
                    break;
                case 7:
                    var arm_json = JsonSerializer.Deserialize<ArmJson>(data);
                    SetArm(arm_json);
                    ServerConfirm(wing_json, true);
                    break;
                case 8:
                    var raise_json = JsonSerializer.Deserialize<RaiseJson>(data);
                    SetRaise(raise_json);
                    ServerConfirm(wing_json, true);
                    break;
                case 9:
                    var goto_json = JsonSerializer.Deserialize<GoToXYThJson>(data);
                    GoToTask(goto_json);
                    ServerConfirm(wing_json, true);
                    break;
                case 10:
                    // 设置绝对坐标，同时相对坐标系也会被刷新
                    var set_odom_json = JsonSerializer.Deserialize<SetOdomJson>(data);
                    if(set_odom_json.data.source == SetOdomFromTmpTf)
                    {
                        // 从临时坐标刷入，应当废弃该方法
                        port.PerformTmpTf();
                    }
                    else
                    {
                        GlobalPosition.x = set_odom_json.data.x;
                        GlobalPosition.y = set_odom_json.data.y;
                        GlobalPosition.yaw = set_odom_json.data.yaw;
                        LocalOrigin.x = 0; //修正坐标后局部原点和全局坐标原点重合
                        LocalOrigin.y = 0;
                        LocalOrigin.yaw = 0;
                        Console.WriteLine(
                            $"Coordinate System is Reset At ({set_odom_json.data.x}, {set_odom_json.data.y}, {set_odom_json.data.yaw})"
                            );
                        port.SetOdomTf(
                            (float)set_odom_json.data.x,
                            (float)set_odom_json.data.y,
                            (float)set_odom_json.data.yaw,
                            method: (byte)WingPort.SetOdomMethod.Absolute
                            ) ;
                        var tmp = KeepYaw;
                        KeepYaw = NormaliseKeepYaw(set_odom_json.data.yaw);
                        Console.WriteLine($"Keep Yaw is Set From {tmp} to {KeepYaw}");
                        /* 把数据结构也刷新
                        port.SetRobotStateOdom(
                            (float)set_odom_json.data.x,
                            (float)set_odom_json.data.y,
                            (float)set_odom_json.data.yaw
                            );
                        */
                    }
                    ServerConfirm(wing_json, true);
                    break;
                default:
                    Console.WriteLine($"Illegae Message Id: {wing_json.id}");
                    break;
            }
        }

        public void ServerConfirm(WingJson wj, bool reply)
        {
            var ack = new AckJson(wj.cnt, reply)
            {
                id = wj.id
            };
            var msg = JsonSerializer.Serialize(ack);
            server.Write(server.GenerateMessage(msg));
        }

        public void ServerFinishNavigation(uint task_id)
        {
            var ack = new AckJson(cnt:task_id, reply:true)
            {
                id = NavigationFinishId //导航任务完成
            };
            var msg = JsonSerializer.Serialize(ack);
            server.Write(server.GenerateMessage(msg));
        }

        public void ServerRetrunState(uint cnt)
        {
            var state = new StateJson()
            {
                id = 1,
                cnt = cnt
            };
            if ((!port.CmdFlags.ClearOdom) && (!port.CmdFlags.SetOdomTf))
            {
                if(StateUpdateCnt!=port.StateUpdateCnt)
                    RefreshGlobalPosition();  //在刷新坐标过程中不刷新全局坐标
            }
            state.data.PidEnable = PidEnable;
            state.data.PidBusy = PidActionBusy;
            state.data.RunMode = (int)RunMode;
            state.data.TeamRole = (int)TeamRole;
            state.data.CtrlMode = port.RobotState.ctrl_mode;
            state.data.TeamDistance = TeamDistance;
            state.data.v = ((int)(port.RobotState.wheel_mps * 1000)) / 1000.0;  //防止小数位数过多
            state.data.vx = port.RobotState.odom.v_liner_x / 1000.0;
            state.data.vy = port.RobotState.odom.v_liner_y / 1000.0;
            state.data.vth = port.RobotState.odom.v_angular_z / 1000.0;
            state.data.x = ReduceAccuracy(GlobalPosition.x);
            state.data.y = ReduceAccuracy(GlobalPosition.y);
            state.data.yaw = ReduceAccuracy(GlobalPosition.yaw);
            state.data.Lx = port.RobotState.odom.x / 1000.0;
            state.data.Ly = port.RobotState.odom.y / 1000.0;
            state.data.Lyaw = ReduceAccuracy(port.RobotState.odom.yaw );
            state.data.KeepYaw = KeepYaw;
            state.data.battery = battery.Percent;
            state.data.ArmState = GetArmState();
            state.data.RaiseState = GetRaiseState();
            state.data.RfidState = port.RobotState.rfid_state;
            state.data.RfidData = GetByteString(port.RobotState.rfid_data);
            var msg = JsonSerializer.Serialize(state);
            server.Write(server.GenerateMessage(msg));
            StateUpdateCnt = port.StateUpdateCnt;
        }

        private int GetArmState()
        {
            int ret = ArmState;
            if(Merged)
            {
                if(ArmState == slave.ArmState)
                {
                    ret = ArmState;
                }
                else
                {
                    ret = slave.ArmState;
                    if(ArmState == (int)ArmStates.Openning || slave.ArmState == (int)ArmStates.Openning)
                    {
                        ret = (int)ArmStates.Openning;
                    }
                    if (ArmState == (int)ArmStates.Closing || slave.ArmState == (int)ArmStates.Closing)
                    {
                        ret = (int)ArmStates.Closing;
                    }
                }
            }

            return ret;
        }

        private int GetRaiseState()
        {
            int ret = RaiseState;
            if(Merged)
            {
                if (RaiseState == slave.RaiseState)
                {
                    ret = RaiseState;
                }
                else
                {
                    ret = slave.RaiseState;
                    if (RaiseState == (int)RaiseStates.MovingUp || slave.RaiseState == (int)RaiseStates.MovingUp)
                    {
                        ret = (int)RaiseStates.MovingUp;
                    }
                    if (RaiseState == (int)RaiseStates.MovingDown || slave.RaiseState == (int)RaiseStates.MovingDown)
                    {
                        ret = (int)RaiseStates.MovingDown;
                    }
                }
            }
            return ret;
        }

        public bool SetRunMode(RunningMode m, double distance)
        {
            if(m == RunMode)
            {
                Console.WriteLine($"Already In Mode: {m}");
                if ( m == RunningMode.SEPARATE_MODE )
                {
                    Console.WriteLine($"TeamDistance Updated: {distance}");
                    TeamDistance = distance;
                    if (distance < 2.0)
                    {
                        Console.WriteLine($"Team Distance Too Small: {distance}, Set 2.5m");
                        TeamDistance = _TeamDistance;
                    }
                }
                return true;
            }
            switch (m)
            {
                case RunningMode.SEPARATE_MODE:
                    TeamDistance = distance;
                    if (Math.Abs(distance) < 2.0)
                    {
                        Console.WriteLine($"Team Distance Too Small: {distance}, Set 2.5m");
                        TeamDistance = _TeamDistance;
                    }
                    if (!port.Ready)
                    {
                        if (!BeginNoPid(timeout: 200))
                        {
                            Console.WriteLine("Can't Connect to Low Level Driver");
                            return false;
                        }
                    }
                    SetDoubleCar(false);
                    UnbindSlave();
                    TogglePid(true);
                    break;
                case RunningMode.MERGE_MODE:
                    if (RunMode!=RunningMode.PARKING_MODE)
                    {
                        Console.WriteLine("Only Parking Mode Can Switch to Merge Mode.");
                        return false;
                    }
                    if (distance < MinTeamDistance)
                    {
                        Console.WriteLine($"Team Distance {distance} Too Small.");
                        return false;
                    }
                    if (TeamRole == TeamPosition.BACK)
                    {
                        Console.WriteLine("I'm Slave Car, Disconnect Stm32.");
                        Dispose();
                        //m = RunningMode.OFFLINE_MODE; //因为后面要把m复制过去，所以这里更新下
                    }
                    else
                    {
                        Console.WriteLine("I'm Master Car, Connecting Slave.");
                        var try_cnt = 3;
                        while (true)
                        {
                            if (!slave.BeginNoPid(timeout: 1000))
                            {
                                Console.WriteLine("Retrying Connect Slave Lanproxy ...");
                            }
                               
                            if(try_cnt-- == 0)
                            {
                                Console.WriteLine("Can't Connect to Slave");
                                return false;
                            }

                            if (slave.port.Ready)
                                break;
                        }
                        this.TeamDistance = distance;
                        slave.TeamDistance = distance;
                        this.TeamRole = TeamPosition.FRONT;
                        this.SetDoubleCar(true);
                        slave.TeamRole = TeamPosition.BACK;
                        slave.SetDoubleCar(true);
                        slave.RunMode = RunningMode.MERGE_MODE;
                        TogglePid(true);
                    }
                    /* 抬车代码 
                    
                    Some Code Here ...

                     */
                    break;
                case RunningMode.OFFLINE_MODE:
                    Dispose();
                    break;
                case RunningMode.PARKING_MODE:
                    // TogglePid(false); 在车底的直线移动还是依赖Pid的, Pid的切换由Parking_node去自动实现
                    if (RunMode == RunningMode.MERGE_MODE)
                    {
                        UnbindSlave();      //可能是从合并模式切回来的，这时候要解绑从车先
                        if (TeamRole == TeamPosition.BACK)
                        {
                            //如果是后车要重新连回stm32
                            var try_cnt = 3;
                            while (true)
                            {
                                if (!BeginNoPid(timeout: 1000))
                                {
                                    Console.WriteLine("Retrying Connect Low Level Driver ...");
                                }

                                if (try_cnt-- == 0)
                                {
                                    Console.WriteLine("Slave Can't Switch From Merge to Parking Mode.");
                                    return false;
                                }

                                if (port.Ready)
                                    break;
                            }
                        }
                    }
                    SetDoubleCar(false);
                    break;
                case RunningMode.DISMISS_MODE:
                    Console.WriteLine($"Team Role: {TeamRole} Switch to Dismiss Mode. Double Car Will Disable.");
                    SetDoubleCar(false);
                    break;
                default:
                    Console.WriteLine("Unkown Mode");
                    return false;
            }
            RunMode = m;
            return true;
        }

        public bool TogglePid(bool s)
        {
            // s和PidEnable 异或必须是真，也就是说不能在已经是使能Pid的时候再使能Pid
            if (!(s ^ PidEnable))
            {
                Console.WriteLine($"Can't Toggle Pid, Set Value {s}, Current Value {PidEnable}");
                return false;
            }

            if (s) StartPid();
            else StopPid();
            return true;
        }

        private void StartPid()
        {
            PidEnable = true;
            if (!(PidThread is null))
            {
                Console.WriteLine("[Error]Cant Start Pid Loop, Maybe Pid is Already Working.");
                return;
            }

            PidThread = new Thread(PidTask);
            PidThread.Start();

        }

        private void StopPid()
        {
            PidEnable = false;
            var timeout = false;
            var t = new System.Timers.Timer(200)
            {
                Enabled = true,
                AutoReset = false
            };
            t.Elapsed += (object sender, ElapsedEventArgs e) =>
            {
                timeout = true;
            };
            while (!(PidThread is null))
            {
                if (timeout)
                {
                    Console.WriteLine("[Error]Time out, Cant Stop Pid Loop");
                    break;
                }
            }
        }

        private void SetTargetSpeed(double vel)
        {
            _TargetVel = vel * RetardingFactor;
        }

        private void SetTargetSpinRpm(short rpm)
        {
            double tmp_rpm = rpm;
            if (Math.Abs(rpm) > PidMaxSpinRpm)
                tmp_rpm = Math.Sign(rpm) * PidMaxSpinRpm;

            if (RunMode == RunningMode.SEPARATE_MODE)
            {
                if (TeamRole == TeamPosition.FRONT)
                {
                    if ((rpm < 0 && AngleDiff < 0) || (rpm > 0 && AngleDiff > 0))
                    {
                        tmp_rpm -= AngleDiff * SpinSlowRatio;
                    }
                }
                else if (TeamRole == TeamPosition.BACK)
                {
                    if ((rpm < 0 && AngleDiff > 0) || (rpm > 0 && AngleDiff < 0))
                    {
                        tmp_rpm += AngleDiff * SpinSlowRatio;
                    }
                }
            }

            if (Math.Sign(tmp_rpm) != Math.Sign(rpm))
                tmp_rpm = 0;
            
            WheelSpinRpm = (short)tmp_rpm;
            //Console.WriteLine($"rpm:{rpm}, AngleDiff:{AngleDiff}, tmp_rpm:{tmp_rpm}");
        }

        private void ChangeOdom(AddOdomJson odom)
        {
            if (odom.data.reset)
            {
                RefreshGlobalPosition();
                LocalOrigin = GlobalPosition;
                ClearOdom();
            }
            else
                port.SetOdomTf(
                    (float)odom.data.x,
                    (float)odom.data.y,
                    (float)odom.data.yaw,
                    method: (byte)WingPort.SetOdomMethod.Incremental
                    );
        }

        public void SetRaise(RaiseJson raise_)
        {
            port.Raise.pump = (byte)(raise_.data.Pump ? 1 : 0);
            port.Raise.up = (byte)(raise_.data.Up ? 1 : 0);
            port.Raise.down = (byte)(raise_.data.Down ? 1 : 0);
            Console.WriteLine($"Team Role: {TeamRole} Set Hydraulic Raise {raise_.data}");

            if (RunMode == RunningMode.MERGE_MODE)
            {
                if (TeamRole == TeamPosition.FRONT)
                {
                    slave.SetRaise(raise_);
                }
            }

            port.SetRaise();
        }

        public void SetArm(ArmJson arm)
        {
            port.Arm.Couples.ArmL.Strech = (byte)(arm.data.Stretch ? 1:0);
            port.Arm.Couples.ArmL.DrawBack = (byte)(arm.data.DrawBack ? 1 : 0);
            port.Arm.Couples.ArmL.Pump = (byte)(arm.data.Pump ? 1 : 0);
            port.Arm.Couples.ArmR = port.Arm.Couples.ArmL;
            Console.WriteLine($"Team Role: {TeamRole} Set Hydraulic Arms {arm.data}");

            if(RunMode == RunningMode.MERGE_MODE)
            {
                if(TeamRole == TeamPosition.FRONT)
                {
                    slave.SetArm(arm);
                }
            }

            port.SetArms();
        }

        private void ClearOdom()
        {
            port.ClearOdom();
            KeepYaw = 0;
            KeepYawOffset = 0.0;
            // 把数据也清空
            // port.ClearRobotStateOdom();
        }

        /// <summary>y
        /// 加减速控制，加速的时候要判断每个轮组位置是否合适
        /// </summary>
        private void SpeedControl()
        {
            /* 如果指令速度和实际速度偏差太大则不进行速度控制，这种方式的风险是如果反馈除了问题，就会失去控制 */
            if (Math.Abs(_CommandVel - CurrentMoveVel) > 0.002) return;
            if (Merged)
            {
                if (Math.Abs(_CommandVel - slave.CurrentMoveVel) > 0.002)
                    return;
            }

            VelErr = CurrentMoveVel - TargetVel;
            double delta_v_2 = VelErr / Math.Abs(VelErr) * VelAcce * 2;

            void update_vel(double acc)
            {
                if (Math.Abs(VelErr) >= acc)
                {
                    _CommandVel = CurrentMoveVel - VelErr / Math.Abs(VelErr) * acc;
                }
                else
                    _CommandVel = TargetVel;
            }

            if (Math.Abs(CurrentMoveVel - delta_v_2) < Math.Abs(CurrentMoveVel))
            {
                /* 减速情况 */
                update_vel(2 * VelAcce);
            }
            else
            {
                if (IsAngleFine())
                {
                    update_vel(VelAcce);
                }
            }

        }

        private void SetSpeed(double vx, double vth)
        {
            SetSpeed(vx, 0, vth);
        }

        public void SetSpeed(double vx, double vy, double vth)
        {
            if (Math.Abs(vx) > 0.8) return;
            if (Math.Abs(vx / vth) < 3.0)
            {
                vth = Math.Sign(vth) * Math.Abs(vx / 3.0);
                if (vx != 0)
                    Debug.Print($"Too Big Vth: {vth}");
            }

            port.SetVelocity(
                    Convert.ToInt16(vx * 1000),
                    Convert.ToInt16(vy * 1000),
                    Convert.ToInt16(vth * 1000)
                );
            if(Merged)
            {
                slave.SetSpeed(vx, vy, vth);
            }
        }

        public void SetSpeed(SpeedJson s)
        {
            switch(s.data.mode)
            {
                case WingPort.CtrlModeMove:
                    SetSpeed(s.data.vx, s.data.vy, s.data.vth);
                    break;
                case WingPort.CtrlModeShift:
                    SetShift(s.data.shiftAng, s.data.shiftMps);
                    break;
                case WingPort.CtrlModeSpin:
                    SetSpin((short)s.data.spinRpm);
                    break;
                default:
                    Console.WriteLine("Error Speed Control Mode.");
                    break;
            }            
        }

        public void SetSpin(short rpm)
        {
            port.SetSpin(rpm);
            if (Merged)
            {
                slave.SetSpin(rpm);
            }
        }

        public void SetShift(double v)
        {
            SetShift(ShiftAngle, v);
        }

        public void SetShift(double angle, double v)
        {
            ShiftAngle = angle;
            port.SetShift(angle, (short)(v * 1000));
            if(Merged)
            {
                slave.SetShift(angle, v);
            }
        }

        public void SetDoubleCar(bool enable)
        {
            DateTime t = DateTime.Now;
            while (true)
            {

                if (!enable)
                {
                    port.SetDoubleCar(false, "None", 0);
                }
                else
                {
                    if (TeamDistance < MinTeamDistance)
                    {
                        Console.WriteLine($"Team Distance Too Small, Set it's value to {_TeamDistance}");
                        TeamDistance = _TeamDistance;
                    }
                    if (TeamRole == TeamPosition.FRONT)
                        port.SetDoubleCar(true, "Front", (uint)(TeamDistance * 1000));
                    else if (TeamRole == TeamPosition.BACK)
                        port.SetDoubleCar(true, "Back", (uint)(TeamDistance * 1000));
                }

                while (true)
                {
                    if (TeamEnable == enable)
                    {
                        Console.WriteLine($"Set Double Car {enable} Successfully.");
                        return;
                    }
                    if ((DateTime.Now - t).TotalSeconds > 1.0)
                    {
                        Console.WriteLine("Set Double Car Time Out, Retrying ...");
                        t = DateTime.Now;
                        break;
                    }
                }
            }
        }

        public void SetDoubleCarInSeperateMode(bool enable)
        {
            if (RunMode != RunningMode.SEPARATE_MODE)
            {
                Console.WriteLine("Please Set Seprate Mode First");
                return;
            }
            SetDoubleCar(enable);
        }

        private bool MoveToPointTask(MoveToPointJson m)
        {
            bool ret = true;
            TaskId = m.data.task_id;
            if (m.data.ManualStop)
            {
                Console.WriteLine("Manually Stop Current Pid Task.");
                AbortTask();
                return true;
            }
            if (PidActionBusy)
            {
                Console.WriteLine("Pid Busy");
                return false;
            }
            if (m.data.RunMode != (int)RunMode)
            {
                Console.WriteLine($"Command RunMode is {m.data.RunMode}, Local Mode is {RunMode}.Please Change Robot Running Mode First.");
                return false;
            }
            if (m.data.dx * m.data.dy != 0)
            {
                Console.WriteLine("Only One Direction Can be Set");
                return false;
            }
            int yaw = (int)Math.Abs(m.data.yaw);
            if ((yaw != 180) && (yaw != 0) && (yaw != 90))
            {
                Console.WriteLine("Yaw Scope is [-90, 0, 90, 180], Please Set Correct Yaw.");
                return false;
            }

            Console.WriteLine($"Start Doing Task:{TaskId}");
            if (m.data.dx == 0 && m.data.dy == 0)
            {
                /* dx和dy都为0表示仅需要旋转
                   维持角度也要刷新 */
                KeepYaw = (int)m.data.yaw;
                SetYaw(m.data.yaw);
                return true;
            }

            /* 直行部分任务 */
            if (Math.Abs(m.data.yaw - port.RobotState.odom.yaw) > 30.0)
            {
                //这里会有bug，比如车子的角度是-179度，发过来的值是180度
                Console.WriteLine("Error Yaw Value, Please Set Yaw First.");
                return false;
            }
            /* yaw = 90, dx !=0 表示要横移，需要完善这种情况
             */
            // WingPy.py程序会安装goTh和bodyTh把移动的正负算出来
            KeepYaw = (int)m.data.yaw;
            if (yaw % 180 == 0)
            {
                SetKeepAxis("X", m.data.y);
                SetMove(m.data.dx);
            }
            else
            {
                SetKeepAxis("Y", m.data.x);
                SetMove(m.data.dy);
            }
            return ret;
        }

        private void GoToTask(GoToXYThJson m)
        {
            if(Math.Abs(m.data.dx)>3 || Math.Abs(m.data.dy)>3 || Math.Abs(m.data.dth)>90)
            {
                Console.WriteLine($"Go To Point {m.data.dx}, {m.data.dy}, {m.data.dth} Out Of Bound");
                return;
            }
            TaskId = m.data.task_id;
            GoToXYTh(m.data.dx, m.data.dy, m.data.dth);
        }

        /// <summary>
        /// 旋转加减速控制
        /// </summary>
        private void SpinControl()
        {
            if (Math.Abs(_CommandSpinRpm - CurrentSpinRpm) > 1) return;
            if(Merged)
            {
                if (Math.Abs(_CommandSpinRpm - slave.CurrentSpinRpm) > 1)
                    return;
            }

            SpinRpmErr = (short)(CurrentSpinRpm - WheelSpinRpm);
            double delta_rpm_2 = 1.0 * SpinRpmErr / Math.Abs(SpinRpmErr) * SpinAcce * 2;
            void update_rpm(short acc)
            {
                if (Math.Abs(SpinRpmErr) >= acc)
                {
                    _CommandSpinRpm = (short)(CurrentSpinRpm - (1.0 * SpinRpmErr / Math.Abs(SpinRpmErr) * acc));
                }
                else
                    _CommandSpinRpm = WheelSpinRpm;
            }

            if (Math.Abs(CurrentSpinRpm - delta_rpm_2) < Math.Abs(CurrentSpinRpm))
            {
                /* 减速情况 */
                update_rpm(2 * SpinAcce);
            }
            else
            {
                if (IsAngleFine())
                {
                    update_rpm(SpinAcce);
                }
            }
        }

        /// <summary>
        /// 斜移加减速控制
        /// </summary>
        private void ShiftControl()
        {
            if (Math.Abs(_CommandVel - CurrentShiftVel) > 0.002) return;
            if (Merged)
            {
                if (Math.Abs(_CommandVel - slave.CurrentShiftVel) > 0.002)
                    return;
            }

            VelErr = CurrentShiftVel - TargetVel;
            double delta_v_2 = VelErr / Math.Abs(VelErr) * VelAcce * 2;

            void update_vel(double acc)
            {
                if (Math.Abs(VelErr) >= acc)
                {
                    _CommandVel = CurrentShiftVel - VelErr / Math.Abs(VelErr) * acc;
                }
                else
                    _CommandVel = TargetVel;
            }

            if (Math.Abs(CurrentShiftVel - delta_v_2) < Math.Abs(CurrentShiftVel))
            {
                /* 减速情况, 斜移的时候需要直接停止，所以斜移的时候速度不能太大 */
                update_vel(VelAcce * 2);
            }
            else
            {
                if (IsAngleFine())
                {
                    update_vel(VelAcce);
                }
            }
        }

        private void SetKeepAxis(string axis_name, double value)
        {
            if ((axis_name != "X") && (axis_name != "Y")) return;
            KeepAxis = axis_name;
            if (axis_name == "X") CenterY = value;
            if (axis_name == "Y") CenterX = value;
        }

        private void DynamicPid_Yaw(double vi)
        {
            double v = Math.Abs(vi);
            if (v <= 0.05)
            {
                YawPid.Kp = 0.25;
                YawPid.Ki = 0.01;
                YawPid.Kd = 1;
                YawPid.EMax = v / (v / 0.05 + 2.0) / Math.PI * 180; //速度越小，转弯半径越越小，角速度越大
            }
            else if (v <= 0.1)
            {
                YawPid.Kp = 0.3;
                YawPid.Ki = 0.01;
                YawPid.Kd = 1.2;
                YawPid.EMax = 0.1 / 3.0 / Math.PI * 180;
            }
            else if (v <= 0.2)
            {
                YawPid.Kp = 0.3;
                YawPid.Ki = 0.01;
                YawPid.Kd = 1.3;
                YawPid.EMax = 0.2 / 3.0 / Math.PI * 180;
            }
            else if (v <= 0.3)
            {
                YawPid.Kp = 0.25;
                YawPid.Ki = 0.005;
                YawPid.Kd = 1.2;
                YawPid.EMax = 0.3 / 4.0 / Math.PI * 180;
            }
            else if (v <= 0.4)
            {
                YawPid.Kp = 0.3;
                YawPid.Ki = 0.0001;
                YawPid.Kd = 1.4;
                YawPid.EMax = 0.4 / 4.0 / Math.PI * 180;
            }
            else
            {
                YawPid.Kp = 0.23;
                YawPid.Ki = 0.001;
                YawPid.Kd = 1.3;
                YawPid.EMax = 0.5 / 4.0 / Math.PI * 180;
            }
        }

        public void DynamicPid_Line(double vi)
        {
            double v = Math.Abs(vi);
            if (v <= 0.1)
            {
                LinePid.Kp = 20;
                LinePid.Kd = 5;
                LinePid.tol = 0.005;
            }
            else if (v <= 0.2)
            {
                LinePid.Kp = 40;
                LinePid.Kd = 1;
                LinePid.tol = 0.005;
            }
            else if (v <= 0.3)
            {
                LinePid.Kp = 30;
                LinePid.Kd = 3.3;
                LinePid.tol = 0.001;
            }
            else if (v <= 0.4)
            {
                LinePid.Kp = 30;
                LinePid.Kd = 4.5;
                LinePid.tol = 0.001;
            }
            else
            {
                LinePid.Kp = 30;
                LinePid.Kd = 5;
                LinePid.tol = 0.001;
            }
        }

        private void PidTask()
        {
            LinePid.EMax = 30;    //轨迹纠偏最大30度修正
            YawPid.tol = 0.1;
            YawPid.Ki = 0.0;

            Console.WriteLine("Start PID LOOP");

            void pid_move()
            {
                if (CtrlMode != WingPort.CtrlModeMove)
                {
                    // 没有准备好
                    SetSpeed(0, 0, 0);
                    return;
                }

                if (TargetVel != 0)
                    SpeedControl();
                else
                    _CommandVel = 0;

                double yaw = port.RobotState.odom.yaw;
                double y = port.RobotState.odom.y / 1000.0;
                double x = port.RobotState.odom.x / 1000.0;

                DynamicPid_Yaw(_CommandVel);
                DynamicPid_Line(_CommandVel);

                if (KeepAxis == "X")
                    LinePidErr.Refresh(CenterY - y);        //Y方向的偏差，这个Y是车子坐标的Y?
                else if (KeepAxis == "Y")
                    LinePidErr.Refresh(CenterX - x);
                else
                    throw new Exception("Wrong Axis Name");
                /* e 的符号与坐标系有关，像素坐标系和标准笛卡尔坐标系正好是反的
                 * 前进：如果在路径左侧e>0，KeepYaw要减小，如果在路径右侧e<0，KeepYaw要增加
                 * 后退：如果在路径左侧e>0，KeepYaw要增加，如果在路径右侧e<0，KeepYaw要减小 
                 * yawPID.Refresh(KeepYaw - yPidSum.GetPIDSum() - yaw);
                 * 
                 * 如果长时间处于一个较大偏差要考虑减速，增加减速的惩罚因子
                 */
                if (Math.Abs(LinePidErr.e2) > LinePid.tol)
                {
                    double sign = 1.0;          //考虑车身姿态180度
                    double align_y = 1.0;       //沿Y方向开的时候在路的左右对应的e的符号和沿X方向开是相反的
                    if (Math.Abs(KeepYaw) == 180) sign = -1.0;
                    if (KeepYaw == -90) sign = -1.0;
                    if (Math.Abs(KeepYaw) == 90) align_y = -1.0;
                    LinePid.P = LinePidErr.e2 * LinePid.Kp * Math.Sign(_CommandVel) * sign * align_y;
                    LinePid.D = LinePidErr.ec * LinePid.Kd * Math.Sign(_CommandVel) * sign * align_y;                    
                }
                else
                {
                    LinePid.P = 0;
                    LinePid.D = 0;
                }

                /* 如果车身方向偏右，yaw_err>0，应当逆时针转（前进的时候左转） */
                if(KeepYaw == 180)
                {
                    if (yaw < 0) yaw += 360;
                }
                else if(KeepYaw == -180)
                {
                    if (yaw > 0) yaw -= 360;
                }
                YawPidErr.Refresh(KeepYaw + KeepYawOffset + LinePid.GetPIDSum() - yaw);
                //YawPidErr.Refresh(KeepYaw - yaw);

                /* 限制下PID控制的最大速度 */
                if (Math.Abs(_CommandVel) > PidMaxVel) _CommandVel = PidMaxVel * _CommandVel / Math.Abs(_CommandVel);
                if (Math.Abs(YawPidErr.e2) > YawPid.tol)
                {
                    YawPid.Ki = 0;
                    YawPid.I = 0;
                    YawPid.P = YawPidErr.e2 * YawPid.Kp;
                    YawPid.D = YawPidErr.ec * YawPid.Kd;
                    SetSpeed(_CommandVel, YawPid.GetPIDSum() / 180.0 * Math.PI);
                }
                else
                {
                    YawPid.P = YawPidErr.e2 * YawPid.Kp;
                    YawPid.D = YawPidErr.ec * YawPid.Kd;
                    if (CurrentMoveVel != 0)
                        YawPid.I += YawPidErr.e2 * YawPid.Ki * 0.01;
                    SetSpeed(_CommandVel, YawPid.GetPIDSum() / 180.0 * Math.PI);
                }
            }

            void pid_spin()
            {
                if (CtrlMode != WingPort.CtrlModeSpin)
                {
                    SetSpin(0);
                    return;
                }
                
                if (WheelSpinRpm != 0)
                    SpinControl();
                else
                    _CommandSpinRpm = 0;
                SetSpin(_CommandSpinRpm);
            }

            void pid_shift()
            {
                double shift_ang;
                if (GoToDx == 0)
                {
                    shift_ang = 90.0 * Math.Sign(GoToDy);
                }
                else
                {
                    shift_ang = Math.Atan(GoToDy / GoToDx) / Math.PI * 180.0;
                }

                if (CtrlMode != WingPort.CtrlModeShift)
                {
                    SetShift(shift_ang, 0);
                    return;
                }

                ShiftControl();
                SetShift(shift_ang, _CommandVel);
            }

            while (port.Ready)
            {
                switch (PidMode)
                {
                    case PidMode_.PID_MOVE:
                        pid_move();
                        break;
                    case PidMode_.PID_SPIN:
                        pid_spin();
                        break;
                    case PidMode_.PID_SHIFT:
                        pid_shift();
                        break;
                    default:
                        break;
                }

                if (!PidEnable)
                    break;

                Thread.Sleep(10);
            }

            Console.WriteLine("Stop Pid Loop");
            PidEnable = false;
            PidThread = null;
        }

        private void AbortTask()
        {
            ManualStop = true;
            switch (PidMode)
            {
                case PidMode_.PID_MOVE:
                case PidMode_.PID_SHIFT:
                    SetTargetSpeed(0);
                    break;
                case PidMode_.PID_SPIN:
                    SetTargetSpinRpm(0);
                    break;
                default:
                    break;
            }
        }

        public void WaitCtrlModeChange(int mode, string msg)
        {
            var _t = new System.Timers.Timer(5000) { AutoReset = true, Enabled = true };
            _t.Elapsed += (object sender, ElapsedEventArgs e) =>
            {
                Console.WriteLine($"Waiting For Changing to {msg} ...");
            };
            while (true)
            {
                if (CtrlMode == mode)
                {
                    Console.WriteLine($"Robot Control Mode Set to {msg} .");
                    _t.Stop();
                    break;
                }
            }
        }

        public void WaitModeValid(byte mode)
        {
            var t = DateTime.Now;
            while (true)
            {
                if (CtrlMode == mode)
                {
                    Console.WriteLine($"SuccessFully Switch Mode {port.StateDict[mode]}");
                    break;
                }

                if (ManualStop)
                    break;

                if ((DateTime.Now - t).TotalSeconds > 2.0)
                {
                    t = DateTime.Now;
                    Console.WriteLine("Still Waiting Mode Changing ...");
                }

            }
        }

        public void WaitStop()
        {
            var t = DateTime.Now;
            while (true)
            {
                if (port.RobotState.wheel_mps == 0)
                {
                    Console.WriteLine($"Robot Stopped.");
                    break;
                }
                if ((DateTime.Now - t).TotalSeconds > 1.0)
                {
                    Console.WriteLine($" Wait Stopping Time Out, Current Wheel Mps {port.RobotState.wheel_mps}.");
                    if (PidMode == PidMode_.PID_SHIFT)
                        SetShift(0);
                    else if (PidMode == PidMode_.PID_SPIN)
                        SetSpin(0);
                    break;
                }

            }
        }

        public void SetMove(double d)
        {
            PidMode = PidMode_.PID_MOVE;
            SetTargetSpeed(0);
            ManualStop = false;
            if (PidActionBusy)
            {
                Console.WriteLine("Pid Moving Busy.");
                return;
            }

            Task.Factory.StartNew(() =>
            {
                switch (RunMode)
                {
                    case RunningMode.OFFLINE_MODE:
                        Console.WriteLine("No Low Level Driver, Mission Ignored.");
                        ManualStop = true;
                        break;
                    case RunningMode.SEPARATE_MODE:
                        if (TeamEnable)
                        {
                            Console.WriteLine("Move In Seperate Mode, Disable Double Car");
                            SetDoubleCarInSeperateMode(false);
                        }
                        break;
                    case RunningMode.MERGE_MODE:
                        if (!TeamEnable)
                        {
                            Console.WriteLine("Move In Merge Mode, But Double Car not Valid.");
                            return;
                        }
                        break;
                    case RunningMode.PARKING_MODE:
                        if (TeamEnable)
                        {
                            Console.WriteLine("Move In Parking Mode, Disable Double Car First!");
                            SetDoubleCar(false);
                        }
                        break;
                    case RunningMode.DISMISS_MODE:
                        if (TeamEnable)
                        {
                            Console.WriteLine("Move In Dismiss Mode, Disable Double Car First!");
                            SetDoubleCar(false);
                        }
                        break;
                    default:
                        Console.WriteLine("Navigation In this Mode is not Supported.");
                        ManualStop = true;
                        break;
                }
                WaitCtrlModeChange(WingPort.CtrlModeMove, "Move Mode");
                PidActionBusy = true;
                double target_len = d;
                double start_x = port.RobotState.odom.x / 1000.0;
                double start_y = port.RobotState.odom.y / 1000.0;
                double current_position;
                double start_position;
                double sign = 1.0; //表明方向
                if (Math.Abs(KeepYaw) == 90)
                {
                    current_position = start_y;
                    start_position = start_y;
                    if (KeepYaw == -90) sign = -1.0;
                }
                else
                {
                    current_position = start_x;
                    start_position = start_x;
                    if (Math.Abs(KeepYaw) == 180) sign = -1.0;
                }

                MovePid.tol = 0.02;
                MovePid.EMax = PidMaxVel;
                MovePid.Kp = 0.6;
                MovePid.Kd = 2.05;
                MovePid.Ki = 0.0;

                Console.WriteLine("Start Pid Move");

                while (!ManualStop)
                {
                    Thread.Sleep(10);
                    if (Math.Abs(KeepYaw) == 90)
                        current_position = port.RobotState.odom.y / 1000.0;
                    else
                        current_position = port.RobotState.odom.x / 1000.0;
                    MovePidErr.Refresh(target_len - sign*(current_position - start_position));
                    if (Math.Abs(MovePidErr.e2) < MovePid.tol)
                    {
                        Console.WriteLine($"Finish, Moved distance: {current_position - start_position}m");
                        SetTargetSpeed(0);
                        break;
                    }
                    else
                    {
                        MovePid.P = MovePidErr.e2 * MovePid.Kp;
                        MovePid.D = MovePidErr.ec * MovePid.Kd;
                    }

                    SetTargetSpeed(MovePid.GetPIDSum());
                }
                SetTargetSpeed(0);

            }).ContinueWith((task) =>
            {
                Console.WriteLine($"Finish Task: {TaskId}");
                ServerFinishNavigation(TaskId);
                PidActionBusy = false;
            });
        }

        public void SetYaw(double ang)
        {
            ManualStop = false;
            if (PidActionBusy)
            {
                Console.WriteLine("Pid Turn Busy.");
                return;
            }        

            Task.Factory.StartNew(() =>
            {

                switch (RunMode)
                {
                    case RunningMode.OFFLINE_MODE:
                        Console.WriteLine("No Low Level Driver, Mission Ignored.");
                        ManualStop = true;
                        break;
                    case RunningMode.SEPARATE_MODE:
                        if (!TeamEnable)
                        {
                            Console.WriteLine("Move In Seperate Mode, Enable Double Car.");
                            SetDoubleCarInSeperateMode(true);
                        }
                        break;
                    case RunningMode.MERGE_MODE:
                        if (!TeamEnable)
                        {
                            Console.WriteLine("Move In Merge Mode, But Double Car not Valid.");
                            return;
                        }
                        break;
                    case RunningMode.DISMISS_MODE:
                    case RunningMode.PARKING_MODE:
                        if (TeamEnable)
                        {
                            Console.WriteLine("Move In Dismiss or Parking Mode, Disable Double Car.");
                            SetDoubleCar(false);
                        }
                        break;
                    default:
                        Console.WriteLine("Navigation In this Mode is not Supported.");
                        ManualStop = true;
                        break;
                }
                /* 先设置双车再设置旋转模式 */
                PidMode = PidMode_.PID_SPIN;
                SetTargetSpinRpm(0);
                WaitCtrlModeChange(WingPort.CtrlModeSpin, "Spin Mode");
                PidActionBusy = true;
                /* 设置的角度是绝对的角度 */
                var target_ang = ang;
                if (Math.Abs(target_ang) > 180)
                {
                    Console.WriteLine("Angle Too Big, Please Use Negative Degrees");
                    return;
                }

                var start_ang = (double)port.RobotState.odom.yaw;
                var current_ang = start_ang;

                TurnPid.tol = 1;
                TurnPid.EMax = PidMaxSpinRpm;     //旋转时电机转速最大值
                TurnPid.Kp = 10;
                TurnPid.Kd = 87.5;
                TurnPid.Ki = 0.0;

                Console.WriteLine("Start Pid Turn");

                double delta_ang(double a1, double a2)
                {
                    // 把偏差角度限定再180以内
                    /** @Change 2021-04-25 放大角度约束，防止出现主车算出来181，从车算出来179
                     * 调整完后发生朝同一个方向转的问题* 
                     */
                    double ret = a1 - a2;
                    if (Math.Abs(ret) > 225)
                    {
                        ret = ret - 360 * ret / Math.Abs(ret);
                    }
                    return ret;
                }

                while (!ManualStop)
                {
                    Thread.Sleep(10);
                    current_ang = port.RobotState.odom.yaw;
                    TurnPidErr.Refresh(delta_ang(target_ang, current_ang));
                    if (Math.Abs(TurnPidErr.e2) < TurnPid.tol)
                    {
                        Console.WriteLine($"Finish, Final Angle: {current_ang}degrees");
                        SetTargetSpinRpm(0);
                        break;
                    }
                    else
                    {
                        TurnPid.P = TurnPidErr.e2 * TurnPid.Kp;
                        TurnPid.D = TurnPidErr.ec * TurnPid.Kd;
                    }

                    SetTargetSpinRpm((short)TurnPid.GetPIDSum());
                }
                SetTargetSpinRpm(0);

            }).ContinueWith((task) =>
            {
                Console.WriteLine("Finish");
                ServerFinishNavigation(TaskId);
                PidActionBusy = false;
            });
        }

        public void GoToXYTh(double dx, double dy, double dth)
        {
            ManualStop = false;
            if (PidActionBusy)
            {
                Console.WriteLine("Pid Moving Busy.");
                return;
            }

            Console.WriteLine($"Start Go To Delta Position: {dx}, {dy}, {dth}.");

            double delta_ang(double a1, double a2)
            {
                // 把偏差角度限定再180以内
                double ret = a1 - a2;
                if (Math.Abs(ret) > 180)
                {
                    ret = ret - 360 * ret / Math.Abs(ret);
                }
                return ret;
            }

            GoToDx = dx;
            GoToDy = dy;
            PidMode = PidMode_.PID_SHIFT;
            Task.Factory.StartNew(() =>
            {
                PidActionBusy = true;
                var start_x = port.RobotState.odom.x / 1000.0;
                var start_y = port.RobotState.odom.y / 1000.0;
                var start_yaw = port.RobotState.odom.yaw;
                var target_distance = Math.Sqrt(dx * dx + dy * dy);
                while (!ManualStop)
                {
                    if (dx == 0 && dy == 0)
                    {
                        Console.WriteLine("No Need Shifting.");
                        break;
                    }
                    Thread.Sleep(10);
                    var sign = dx >= 0 ? 1.0 : -1;
                    SetTargetSpeed(sign * MaxGoToVel);
                    var err_x = port.RobotState.odom.x / 1000.0 - start_x;
                    var err_y = port.RobotState.odom.y / 1000.0 - start_y;
                    var err_distance = Math.Sqrt(err_x * err_x + err_y * err_y);
                    if (Math.Abs(err_distance - target_distance) < 0.02 || err_distance >= target_distance)
                    {
                        SetTargetSpeed(0);
                        WaitStop();
                        Console.WriteLine($"Get Point, Dx: {err_x}, Dy: {err_y}, Distance: {err_distance}");
                        break;
                    }
                }

                if (dth == 0)
                {
                    Console.WriteLine("No Need Spinning");
                    return;
                }

                Console.WriteLine($"Shift Finish, Start Spinning, TeamEnable: {port.RobotState.team_info.enable}");
                PidMode = PidMode_.PID_SPIN;
                WaitModeValid(WingPort.CtrlModeSpin);

                while (!ManualStop)
                {
                    var err_th = delta_ang(port.RobotState.odom.yaw, start_yaw);
                    if (Math.Abs(err_th - dth) < 0.2 || Math.Abs(err_th) > Math.Abs(dth))
                    {
                        WheelSpinRpm = 0;
                        WaitStop();
                        Console.WriteLine($"Get Point, Dth: {err_th}");
                        Console.WriteLine("Finish, Switching Back To Pid Move Mode.");
                        PidMode = PidMode_.PID_MOVE;
                        WaitModeValid(WingPort.CtrlModeMove);
                        err_th = delta_ang(port.RobotState.odom.yaw, start_yaw);
                        Console.WriteLine($"After Switching To Mode Mode, Dth: {err_th}");
                        break;
                    }
                    WheelSpinRpm = (short)(Math.Sign(dth) * 50);
                }

            }).ContinueWith((task) =>
            {
                SetTargetSpeed(0);
                PidMode = PidMode_.PID_MOVE;
                WaitModeValid(WingPort.CtrlModeMove);
                Console.WriteLine("Finish.");
                ServerFinishNavigation(TaskId);
                PidActionBusy = false;
            });
        }

    }
}
